Added: beam-skipping to likelihood-field-prob - #577
Conversation
glpuga
left a comment
glpuga
left a comment
There was a problem hiding this comment.
It's a great first pass @ralcoberro
| const auto pz = this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob); | ||
| // The likelihood field is monotonically decreasing in the distance to the nearest | ||
| // obstacle, so "distance < beam_skip_distance" is equivalent to "pz > threshold". | ||
| if (pz > likelihood_threshold_) { | ||
| ++obs_count[i]; | ||
| } |
There was a problem hiding this comment.
It's a bit unfortunate that we need to use the probability as a proxy for distance, because it will not be correct. That is mainly because there exists a laser_likelihood_max_dist that sets a maximum distance to calculate the likelihood field map, and for distances larger than that we use a flat value.
That means if the if the beam_skip_distance > laser_likelihood_max_dist , we will count all rays that hit beyond laser_likelihood_max_dist as being "in".
We don't have a distance map, which would solve this, but I think that's good because having one would bring trouble:
- Double the memory usage for large maps.
- laser_likelihood_max_dist exists because for most real-world maps it reduces the time it takes to calculate the distance map to very small fraction because most of the area of the map is void areas you don't care the likelihood in any case.
Both issues probably have other solutions that the ones currently in use, but that would mean a deviation from the current behavior of nav2.
We can discuss it, though.
There was a problem hiding this comment.
Thank you Gera, let's talk about this
There was a problem hiding this comment.
let me know when you want to talk about this.
There was a problem hiding this comment.
To unblock this, given that th beam skip distance will usually be much smaller than laser_likelihood_distance, and so are their default values, lets declare that laser_likelihood_distance > beam_skip_distance is a requirement of the configuration. Also add an assertion in the beamskipping code itself enforcing that constraint.
In https://github.com/Ekumen-OS/beluga/blob/main/beluga_amcl/src/amcl_node.cpp#L376-L395 add a check that if beam skipping is enabled, laser_likelihood_max_dist must be greater or equal to beam_skip_distance. If it is not, we throw to let the user know.
Also, in https://github.com/Ekumen-OS/beluga/blob/feature/beam-skipping-likelihood-field-prob/beluga_amcl/src/amcl_node.cpp#L199C24-L199C42 and in the documentation entry for beam_skip_distance make that constraint explicit.
There are alternatives, but they will require a lot of refactoring to the make_likelihood_field function, and in practice will have the same computational cost as making laser_likelihood_distance == beam_skip_distance, because that's the only way not to have to create more than one copy of the distance map.
| double log_weight = 0.0; | ||
| for (std::size_t i = 0; i < points.size(); ++i) { | ||
| // Skip beams that were masked out by prepare(). When beam skipping is disabled (or | ||
| // prepare() was never called) the mask is not consulted and every beam contributes, | ||
| // reproducing the plain likelihood field prob behavior. | ||
| if (do_beamskip_ && i < beam_mask_.size() && !beam_mask_[i]) { | ||
| continue; | ||
| } | ||
| // Transform the end point of the laser to the grid local coordinate system. | ||
| // Not using Eigen/Sophus because they make the routine x10 slower. | ||
| // See `benchmark_likelihood_field_model.cpp` for reference. | ||
| const auto& point = points[i]; | ||
| const auto x = point.first * cos_theta - point.second * sin_theta + x_offset; | ||
| const auto y = point.first * sin_theta + point.second * cos_theta + y_offset; | ||
| const auto pz = | ||
| static_cast<double>(this->likelihood_field_.data_near(x, y).value_or(unknown_space_occupancy_prob)); | ||
| log_weight += std::log(pz); | ||
| } | ||
| return std::exp(log_weight); |
There was a problem hiding this comment.
You can do this without resorting back to for loops and at the same time reduce the amount of changes by zipping together the beam_mask and the points vectors, and returning 0.0 from the lambda if the beam is masked. You don't need to check do_beamskip_ because the mask will be true for all beams in that case anyways.,
There was a problem hiding this comment.
|
@griswaldbrooks , this is the WIP beam-skipping feature that @ralcoberro has been working on that we mentioned last week. |
3 participants
Proposed changes
Draft of the beam skipping feature, companion to the likelihood_field_prob sensor model .
Type of change
💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.
Checklist
Put an
xin the boxes that apply. This is simply a reminder of what we will require before merging your code.Additional comments
Anything worth mentioning to the reviewers.